﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using WiimoteLib;

namespace IndoorPositionFinder
{
    public class RoomObject
    {
        private String objectName;
        private List<PointF> cornerPoints;

        public RoomObject(String name, List<PointF> cornerPoints)
        {
            this.objectName = name;
            this.cornerPoints = cornerPoints;
        }
        #region Getters & Setters
        public String Name { get { return objectName; } set { objectName = value; } }
        public List<PointF> CornerPoints { get { return cornerPoints; } set { cornerPoints = value; } }
        #endregion
        /// <summary>
        /// Calculate the centroid associated with the object
        /// </summary>
        /// <returns>The point that represent the centroid</returns>
        public PointF centroid()
        {
            PointF centroid = new PointF();
            foreach (PointF point in cornerPoints)
            {
                centroid.X += point.X;
                centroid.Y += point.Y;
            }
            centroid.X = centroid.X / (float)cornerPoints.Count;
            centroid.Y = centroid.Y / (float)cornerPoints.Count;
            return centroid;
        }
        /// <summary>
        /// Calculate the euclidian distance between the user position and the object.
        /// The point that represent the object is the centroid
        /// </summary>
        /// <param name="userPosition"></param>
        /// <returns>distance between the object an the user</returns>
        public float distance(PointF userPosition)
        {
            float distance = 0;
            PointF centroid = this.centroid();
            distance = (float)Math.Sqrt((double)(Math.Pow((double)(userPosition.X - centroid.X), 2) + Math.Pow((double)(userPosition.Y - centroid.Y), 2)));
            return distance;
        }
        /// <summary>
        /// Returns the "angle" between the user and the object, relative to the
        /// user's movement vector following the clock representation paradigm.
        /// </summary>
        /// <param name="movementVector"></param>
        /// <returns>Room's position relative to the user and especified in a clock schema</returns>
        public float relativeAngle(PointF userPosition, PointF movementVector)
        {
            PointF toCentroidVector = new PointF();
            toCentroidVector.X = this.centroid().X - userPosition.X;
            toCentroidVector.Y = this.centroid().Y - userPosition.Y;
            // Now we calculate the angle between the vectors using the dot product
            float dotProduct = toCentroidVector.X * movementVector.X + toCentroidVector.Y * movementVector.Y;
            float toCentroidNorm = (float)Math.Sqrt(Math.Pow((double)toCentroidVector.X, 2) + Math.Pow((double)toCentroidVector.Y, 2));
            float movementVectorNorm = (float)Math.Sqrt(Math.Pow((double)movementVector.X, 2) + Math.Pow((double)movementVector.Y, 2));
            // shortest angle between the vectors
            float cosAngle = (dotProduct / (toCentroidNorm * movementVectorNorm));
            float relativeAngle = (float)Math.Acos((double)cosAngle);
            // Now we need to convert the angle in the "hour format" and if we want to do that
            // we need to change the convention to define the angle between two vector to one according to the
            // definition of hours in a clock

            // Defining the interesting cases. Generally we care about the case that occurs when the
            // movementVector points to the opposite section than the toCentroidVector

            // if the movement vector points to the first section of the plane...
            if (movementVector.X >= 0 && movementVector.Y >= 0)
            {
                // If the toCentroidVector points to the third section
                if (toCentroidVector.X < 0 && toCentroidVector.Y < 0)
                {
                    // Calculating and comparing the angles between the vectors and the X axis
                    float movementAngle = (float)Math.Atan((double)(movementVector.Y / movementVector.X));
                    float toCentroidAngle = (float)Math.Atan((double)Math.Abs(toCentroidVector.Y) / (double)Math.Abs(toCentroidVector.X));
                    // if the movementAngle is bigger than the toCentroidAngle, 
                    // then I change the angle to its complement
                    if (movementAngle > toCentroidAngle)
                    {
                        relativeAngle = 2f * (float)Math.PI - relativeAngle;
                    }

                }
                // If the centroid vector points to the fourth quadrant
                if (toCentroidVector.X < 0 && toCentroidVector.Y >= 0)
                {
                    relativeAngle = 2f * (float)Math.PI - relativeAngle;
                }
                // If the centroid vector points to the first quadrant...
                if (toCentroidVector.X >= 0 && toCentroidVector.Y >= 0)
                {
                    //...we need to check if the toCentroidAngle is on left or right 
                    // from the movementVector. To check that we calculate the angle between
                    // the vectors and the X axis
                    float movementAngle = (float)Math.Atan((double)(movementVector.Y / movementVector.X));
                    float toCentroidAngle = (float)Math.Atan((double)(toCentroidVector.Y / toCentroidVector.X));
                    if (toCentroidAngle > movementAngle)
                    {
                        relativeAngle = 2f * (float)Math.PI - relativeAngle;
                    }

                }
            }
            // Now we do the same for the rest of cases...
            // ...if the movement vector points to the second quadrant
            if (movementVector.X >= 0 && movementVector.Y < 0)
            {
                if (toCentroidVector.X < 0 && toCentroidVector.Y >= 0)
                {
                    // calculating the angles between the vectors and the X-Axis
                    float movementAngle = (float)Math.Atan((double)(Math.Abs(movementVector.Y) / movementVector.X));
                    float toCentroidAngle = (float)Math.Atan((double)(toCentroidVector.Y / Math.Abs(toCentroidVector.X)));
                    if (movementAngle < toCentroidAngle)
                    {
                        relativeAngle = 2f * (float)Math.PI - relativeAngle;
                    }
                }
                if (toCentroidVector.X >= 0 && toCentroidVector.Y >= 0)
                {
                    relativeAngle = 2f * (float)Math.PI - relativeAngle;
                }
                if (toCentroidVector.X >= 0 && toCentroidVector.Y < 0)
                {
                    // calculating the angles between the vectors and the X-Axis
                    float movementAngle = (float)Math.Atan((double)(Math.Abs(movementVector.Y) / movementVector.X));
                    float toCentroidAngle = (float)Math.Atan((double)(Math.Abs(toCentroidVector.Y) / toCentroidVector.X));
                    if (movementAngle >= toCentroidAngle)
                    {
                        relativeAngle = 2f * (float)Math.PI - relativeAngle;
                    }
                }
            }
            // ...if the movement vector points to the third quadrant
            if (movementVector.X < 0 && movementVector.Y < 0)
            {
                if (toCentroidVector.X >= 0 && toCentroidVector.Y >= 0)
                {
                    // calculating the angles between the vectors and the X-Axis
                    float movementAngle = (float)Math.Atan((double)(Math.Abs(movementVector.Y) / Math.Abs(movementVector.X)));
                    float toCentroidAngle = (float)Math.Atan((double)(toCentroidVector.Y / toCentroidVector.X));
                    if (movementAngle >= toCentroidAngle)
                    {
                        relativeAngle = 2f * (float)Math.PI - relativeAngle;
                    }
                }
                if (toCentroidVector.X >= 0 && toCentroidVector.Y < 0)
                {
                    relativeAngle = 2f * (float)Math.PI - relativeAngle;
                }
                if (toCentroidVector.X < 0 && toCentroidVector.Y < 0)
                {
                    // calculating the angles between the vectors and the X-Axis
                    float movementAngle = (float)Math.Atan((double)(Math.Abs(movementVector.Y) / Math.Abs(movementVector.X)));
                    float toCentroidAngle = (float)Math.Atan((double)(Math.Abs(toCentroidVector.Y) / Math.Abs(toCentroidVector.X)));
                    if (movementAngle < toCentroidAngle)
                    {
                        relativeAngle = 2f * (float)Math.PI - relativeAngle;
                    }
                }
            }
            // ...if the movement vector points to the fourth quadrant
            if (movementVector.X < 0 && movementVector.Y >= 0)
            {
                if (toCentroidVector.X >= 0 && toCentroidVector.Y < 0)
                {
                    // calculating the angles between the vectors and the X-Axis
                    float movementAngle = (float)Math.Atan((double)(movementVector.Y / Math.Abs(movementVector.X)));
                    float toCentroidAngle = (float)Math.Atan((double)(Math.Abs(toCentroidVector.Y) / toCentroidVector.X));
                    if (movementAngle < toCentroidAngle)
                    {
                        relativeAngle = 2f * (float)Math.PI - relativeAngle;
                    }
                }
                if (toCentroidVector.X < 0 && toCentroidVector.Y < 0)
                {
                    relativeAngle = 2f * (float)Math.PI - relativeAngle;
                }
                if (toCentroidVector.X < 0 && toCentroidVector.Y >= 0)
                {
                    // calculating the angles between the vectors and the X-Axis
                    float movementAngle = (float)Math.Atan((double)(movementVector.Y / Math.Abs(movementVector.X)));
                    float toCentroidAngle = (float)Math.Atan((double)(toCentroidVector.Y / Math.Abs(toCentroidVector.X)));
                    if (movementAngle >= toCentroidAngle)
                    {
                        relativeAngle = 2f * (float)Math.PI - relativeAngle;
                    }
                }
            }
            // Finally we change the angle for an "hour" angle
            float hourAngle = angleToHour(relativeAngle);
            return hourAngle;
        }
        /// <summary>
        /// Convert the given radian angle into an "hour angle"
        /// </summary>
        /// <param name="angle">Angle in radians</param>
        /// <returns>float that represents angle angle in hour format</returns>
        private float angleToHour(float angle)
        {
            //
            float hourExtension = 2f * (float)Math.PI / 12f;
            float hour = 0;
            if (angle >= (hourExtension / 2f) && angle < 2f * Math.PI - (hourExtension / 2f))
                hour = (float)Math.Round((double)(angle / hourExtension));
            else
                hour = 12f;
            return hour;
        }
        public bool toClose(float warningDistance, PointF userPosition)
        {
            bool toClose = false;
            // Determines the warning area coordinates.
            List<PointF> warningCornerPoints = new List<PointF>();
            PointF centroid = this.centroid();
            foreach (PointF cornerPoint in this.cornerPoints)
            {
                PointF warningCornerPoint = new PointF();

                if (cornerPoint.X >= centroid.X)
                {
                    // Case: First quadrant
                    if (cornerPoint.Y >= centroid.Y)
                    {
                        warningCornerPoint.X = cornerPoint.X + warningDistance;
                        warningCornerPoint.Y = cornerPoint.Y + warningDistance;
                    }
                    // Case: Second Quadrant
                    else
                    {
                        warningCornerPoint.X = cornerPoint.X + warningDistance;
                        warningCornerPoint.Y = cornerPoint.Y - warningDistance;
                    }
                }
                else
                {
                    // Case: Third quadrant
                    if (cornerPoint.Y < centroid.Y)
                    {
                        warningCornerPoint.X = cornerPoint.X - warningDistance;
                        warningCornerPoint.Y = cornerPoint.Y - warningDistance;
                    }
                    // Case: Fourth Quadrant
                    else
                    {
                        warningCornerPoint.X = cornerPoint.X - warningDistance;
                        warningCornerPoint.Y = cornerPoint.Y + warningDistance;
                    }
                }
                warningCornerPoints.Add(warningCornerPoint);

            }
            // Check if the userPosition is into the warning area
            toClose = this.pointInPolygon(warningCornerPoints, userPosition);
            // return the results
            return toClose;
        }
        private bool pointInPolygon(List<PointF> polygonVert, PointF point)
        {
            int i, j;
            bool c = false;
            int nvert = polygonVert.Count;
            for (i = 0, j = nvert - 1; i < nvert; j = i++)
            {
                if (((polygonVert.ElementAt(i).Y > point.Y) != (polygonVert.ElementAt(j).Y > point.Y)) &&
                 (point.X < (polygonVert.ElementAt(j).X - polygonVert.ElementAt(i).X) * 
                 (point.Y - polygonVert.ElementAt(i).Y) / (polygonVert.ElementAt(j).Y - polygonVert.ElementAt(i).Y) + polygonVert.ElementAt(i).X))
                    c = !c;
            }
            return c;
        }

    }
}
